//
//  main.cpp
//  opencvtest
//
//  Created by 李勇 on 2019/4/27.
//  Copyright © 2019 李勇. All rights reserved.
//

#include <iostream>
#include <opencv2/opencv.hpp>
#include <string>

using namespace cv;
using namespace std;

/*运动估计,光流估计*/
#if 1
#define MAX_POINT_COUNT 50

int main(int argc, const char * argv[])
{
    //char *fn = "/Users/liyong/program/opencv/samples/data/vtest.avi";
    string fn("/Users/liyong/program/opencv/samples/data/vtest.avi"); // by default
    if( argc > 1)
    {
        fn = argv[1];
    }
    
    VideoCapture cap;
    Mat source, result, gray, lastGray;//对应本帧和上一帧的灰度图
    vector<Point2f> points[2], temp; //对应上一帧和本帧的特征点，上一帧是给定的，本帧时预测结果
    vector<uchar> status; //每一个特征点检测状态
    vector<float> err; //每一个特征点计算误差
    
    cap.open(fn.c_str());
    if(!cap.isOpened())
    {
        cout << "无法打开视频，请检查文件或摄像头" << endl;
        return 1;
    }
    
    for (;;)
    {
        cap >> source;
        if(source.empty())
            break;
        
        cvtColor(source, gray, COLOR_BGR2GRAY);
        
        if(points[0].size() < 10)
        {
            goodFeaturesToTrack(gray, points[0], MAX_POINT_COUNT, 0.01, 20);
        }
        
        if (lastGray.empty())
            gray.copyTo(lastGray);
        
        calcOpticalFlowPyrLK(lastGray, gray, points[0], points[1], status, err);
        
        //删除误判点
        int counter = 0;
        for (int i = 0; i < points[1].size(); i++)
        {
            double dist = norm(points[1][i] - points[0][i]);
            if (status[i] && dist >= 2.0 && dist <= 20.0)
            {
                points[0][counter] = points[0][i];
                points[1][counter++] = points[1][i];
            }
        }
        
        points[0].resize(counter);
        points[1].resize(counter);
        
        //显示特征点和运动轨迹
        source.copyTo(result);
        for (int i = 0; i < points[1].size(); i++)
        {
            line(result, points[0][i], points[1][i], Scalar(0, 0, 0xff));
            circle(result, points[1][i], 3, Scalar(0, 0xff, 0));
        }
        
        swap(points[0], points[1]);
        swap(lastGray, gray);
        
        imshow("sorce", source);
        imshow("result", result);
        
        /*以下检测是否终止（按下esc终止，对应ascii 27）*/
        char key = waitKey(100);
        if(27 == key)
            break;
        
    }
    
}
#endif
